#include "can_interface.h"
#include "io_macro.h"
#include <rclcpp/clock.hpp>
#include <rclcpp/utilities.hpp>

namespace io_module {

CanInterface::CanInterface(
    std::shared_ptr<rclcpp::Node>& node,
    const ModuleParam& param)
: BaseInterface(node, param)
{
  do_byte_pubs_.resize(MAX_CHANNEL);
  di_byte_subs_.resize(MAX_CHANNEL);
  ao_word_pubs_.resize(MAX_ANALOG_CHANNEL);
  ai_word_subs_.resize(MAX_ANALOG_CHANNEL);
}

CanInterface::~CanInterface() {
  // 显式释放资源
  do_byte_pubs_.clear();
  di_byte_subs_.clear();
  ao_word_pubs_.clear();
  ai_word_subs_.clear();
}

bool CanInterface::init() {
  try {
    if(m_moduleParam.node_id == 0 || m_moduleParam.can_port.empty()) {
      DEBUG_ERROR_OUT("Invalid CAN parameters: node_id=" << m_moduleParam.node_id << ",port=" << m_moduleParam.can_port);
      return false;
    }

    DEBUG_OUT("CAN%d initialized with %d digital I/O and %d analog I/O",
               m_moduleParam.node_id,
               m_moduleParam.num_digital_input + m_moduleParam.num_digital_output,
               m_moduleParam.num_analog_input + m_moduleParam.num_analog_output);
    initDIoTopicParam();
    initAIoTopicParam();
    return true;
  } catch (const std::exception& e) {
    DEBUG_ERROR_OUT("Initialization failed: " << e.what());
    return false;
  }
}


void CanInterface::initDIoTopicParam()
{
  if (do_byte_pubs_.size() < 8 || di_byte_subs_.size() < 8) {
    DEBUG_ERROR_OUT("initDIoTopicParam failed");
    return;
  }

  digital8<0>();
  digital8<1>();
  digital8<2>();
  digital8<3>();
  digital8<4>();
  digital8<5>();
  digital8<6>();
  digital8<7>();
}

void CanInterface::initAIoTopicParam()
{
  if (ao_word_pubs_.size() < 16 || ai_word_subs_.size() < 16) {
      DEBUG_ERROR_OUT("initAIoTopicParam failed");
      return;
  }
  analog16<0>();
  analog16<1>();
  analog16<2>();
  analog16<3>();
  analog16<4>();
  analog16<5>();
  analog16<6>();
  analog16<7>();
  analog16<8>();
  analog16<9>();
  analog16<10>();
  analog16<11>();
  analog16<12>();
  analog16<13>();
  analog16<14>();
  analog16<15>();
}

template <size_t Index>
void CanInterface::digital8() 
{
  const auto qos = rclcpp::QoS(10);
  
  // 数字输出发布者
  const std::string do_topic = 
    "/" + m_moduleParam.can_port + 
    "/device" + std::to_string(m_moduleParam.node_id) +
    "/digoutput8_" + std::to_string(Index+1);
  
  do_byte_pubs_[Index] = node_->create_publisher<std_msgs::msg::UInt8>(
    do_topic,
    qos
  );

  // 数字输入订阅者
  const std::string di_topic = 
    "/" + m_moduleParam.can_port + 
    "/device" + std::to_string(m_moduleParam.node_id) +
    "/diginput8_" + std::to_string(Index+1);
  
  di_byte_subs_[Index] = node_->create_subscription<std_msgs::msg::UInt8>(
    di_topic,
    qos,
    [this](std_msgs::msg::UInt8::ConstSharedPtr msg) {
      this->di_byte_sub_callback<Index>(msg);
    }
  );
}

template <size_t Index>
void CanInterface::di_byte_sub_callback(std_msgs::msg::UInt8::ConstSharedPtr msg) 
{
  if constexpr (Index >= MAX_CHANNEL) {
    DEBUG_ERROR_OUT("Digital channel index overflow: " << Index);
    return;
  }
  
  const uint8_t new_value = msg->data;
  const uint8_t old_value = di_bytes[Index].exchange(new_value);
  // if(new_value != old_value) {
  //   DEBUG_WARN_OUT(
  //     "DI:" << Index 
  //     << " updated:0x" << 
  //     std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(old_value) 
  //     << "->0x" << 
  //     std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(new_value));
  // }
  updateTime();
}

bool CanInterface::setOutput(const uint8_t& index, const bool& level) 
{
  // 参数校验
  if(index >= m_moduleParam.num_digital_output) {
    DEBUG_ERROR_OUT("DO index " << index << " out of range (0-0x" << 
        std::hex << m_moduleParam.num_digital_output-1 << ")");
    return false;
  }

  // 计算通道和位偏移
  const uint8_t channel = index / 8;
  const uint8_t bit = index % 8;

  // 原子操作更新输出值
  uint8_t expected = do_bytes[channel].load();
  uint8_t desired;
  do {
    desired = level ? (expected | (1 << bit)) : (expected & ~(1 << bit));
  } while(!do_bytes[channel].compare_exchange_weak(expected, desired));

  // 发布更新
  auto msg = std_msgs::msg::UInt8();
  msg.data = desired;
  do_byte_pubs_[channel]->publish(msg);

  DEBUG_OUT("Set DO%d.%d to %d", channel, bit, level);
  return true;
}

bool CanInterface::readInput(uint64_t& result) 
{
  bool data_timeout = dataTimeOut();
  DEBUG_WARN_ON_TRANSITION(data_timeout, "error: canopen digtal data time out number=" << m_moduleParam.module_number);
  if(data_timeout) {
    return false;
  }

  // 组装64位数据
  result = 0;
  const uint8_t channels = m_moduleParam.num_digital_input / 8;
  for(uint8_t i = 0; i < channels; ++i) {
    result |= static_cast<uint64_t>(di_bytes[i].load()) << (i * 8);
  }
  return true;
}

template <size_t Index>
void CanInterface::analog16() 
{
  const auto qos = rclcpp::QoS(10);

  // 模拟量输出发布者
  const std::string ao_topic = 
    "/" + m_moduleParam.can_port + 
    "/device" + std::to_string(m_moduleParam.node_id) +
    "/analog_output_" + std::to_string(Index+1);
  
  ao_word_pubs_[Index] = node_->create_publisher<std_msgs::msg::UInt16>(
    ao_topic,
    qos
  );

  // 模拟量输入订阅者
  const std::string ai_topic = 
    "/" + m_moduleParam.can_port + 
    "/device" + std::to_string(m_moduleParam.node_id) +
    "/analog_input_" + std::to_string(Index+1);
  
  ai_word_subs_[Index] = node_->create_subscription<std_msgs::msg::UInt16>(
    ai_topic,
    qos,
    [this](std_msgs::msg::UInt16::ConstSharedPtr msg) {
      this->ai_word_sub_callback<Index>(msg);
    }
  );
}

template <size_t Index>
void CanInterface::ai_word_sub_callback(std_msgs::msg::UInt16::ConstSharedPtr msg) 
{
  if constexpr (Index >= MAX_ANALOG_CHANNEL) {
    DEBUG_ERROR_OUT("Analog channel index overflow: " << Index);
    return;
  }

  const uint16_t new_value = msg->data;
  const uint16_t old_value = ai_words[Index].exchange(new_value);
  
  if(new_value != old_value) {
    DEBUG_ERROR_OUT("AI" << Index << "updated: 0x" << std::hex << old_value << "-> 0x" << std::hex << new_value);
    updateTimeA();
  }
}

bool CanInterface::setOutputA(const uint8_t& index, const WORD& rawData) 
{
  if(index >= m_moduleParam.num_analog_output) {
    DEBUG_ERROR_OUT("AO index " << index << " out of range (0-0x" 
                << std::hex << m_moduleParam.num_analog_output-1 << ")");
    return false;
  }

  // 更新并发布数据
  ao_words[index].store(rawData);
  
  auto msg = std_msgs::msg::UInt16();
  msg.data = rawData;
  ao_word_pubs_[index]->publish(msg);
  
  DEBUG_OUT("Set AO%d to %u", index, rawData);
  return true;
}

bool CanInterface::readInputA(std::vector<WORD>& result)
{
  bool analog_timeout = dataTimeOutA();
  DEBUG_WARN_ON_TRANSITION(analog_timeout, "Analog inputs data expired");
  if(analog_timeout) {
    return false;
  }

  result.clear();
  for(uint8_t i = 0; i < m_moduleParam.num_analog_input; ++i) {
    result.emplace_back(ai_words[i].load());
  }
  return true;
}

} // namespace io_module